﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;

namespace TSL
{
    class KinectModule
    {
        KinectSensor kin;
        bool closing = false;
        const int skeletonCount = 6;
        const float movingThreshold = 0.005f;
        Skeleton[] allSkeletons = new Skeleton[skeletonCount];
        Skeleton first;
        Joint head, handLeft, handRight, spine, shoulderLeft, shoulderRight, hipLeft, hipRight;
        double leftX, leftY, leftZ;
        double rightX, rightY, rightZ;
        double headX, headY, headZ;
        double shoulderLX, shoulderLY, shoulderLZ, shoulderRX, shoulderRY, shoulderRZ;
        double hipLX, hipLY, hipLZ, hipRX, hipRY, hipRZ;

        Joint oldHandLeft, oldHandRight;
        Joint oldHead, oldSpine, oldShoulderLeft, oldShoulderRight, oldHipLeft, oldHipRight;
        int movingTimeOut = 0;

        public KinectModule(KinectSensor k)
        {
            this.kin = k;
            kin.SkeletonStream.Enable();
            kin.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(kin_AllFramesReady);
        }
        void kin_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            if (closing)
            {
                return;
            }

            first = GetFirstSkeleton(e);

            if (first == null)
            {
                return;
            }
            else
            {
                //Binding Joint
                head = first.Joints[JointType.Head]; spine = first.Joints[JointType.Spine];
                handLeft = first.Joints[JointType.HandLeft]; handRight = first.Joints[JointType.HandRight];
                shoulderLeft = first.Joints[JointType.ShoulderLeft]; shoulderRight = first.Joints[JointType.ShoulderRight];
                hipLeft = first.Joints[JointType.HipLeft]; hipRight = first.Joints[JointType.HipRight];

                //get Position
                headX = head.Position.X - spine.Position.X; headY = head.Position.Y - spine.Position.Y; headZ = head.Position.Z - spine.Position.Z;
                leftX = handLeft.Position.X - spine.Position.X; leftY = handLeft.Position.Y - spine.Position.Y; leftZ = handLeft.Position.Z - spine.Position.Z;
                rightX = handRight.Position.X - spine.Position.X; rightY = handRight.Position.Y - spine.Position.Y; rightZ = handRight.Position.Z - spine.Position.Z;
                shoulderLX = shoulderLeft.Position.X - spine.Position.X; shoulderLY = shoulderLeft.Position.Y - spine.Position.Y; shoulderLZ = shoulderLeft.Position.Z - spine.Position.Z;
                shoulderRX = shoulderRight.Position.X - spine.Position.X; shoulderRY = shoulderRight.Position.Y - spine.Position.Y; shoulderRZ = shoulderRight.Position.Z - spine.Position.Z;
                hipLX = hipLeft.Position.X - spine.Position.X; hipLY = hipLeft.Position.Y - spine.Position.Y; hipLZ = hipLeft.Position.Z - spine.Position.Z;
                hipRX = hipRight.Position.X - spine.Position.X; hipRY = hipRight.Position.Y - spine.Position.Y; hipRZ = hipRight.Position.Z - spine.Position.Z; 
            }

            
        }

        Skeleton GetFirstSkeleton(AllFramesReadyEventArgs e)
        {
            using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())
            {
                if (skeletonFrameData == null)
                {
                    return null;
                }

                skeletonFrameData.CopySkeletonDataTo(allSkeletons);

                Skeleton first = (from s in allSkeletons
                                  where s.TrackingState == SkeletonTrackingState.Tracked
                                  select s).FirstOrDefault();
                return first;

            }
        }

        public bool CheckHandMoving()
        {
            if ((oldHandLeft.Position.X - handLeft.Position.X) >= movingThreshold || (oldHandLeft.Position.Y - handLeft.Position.Y) >= movingThreshold || (oldHandLeft.Position.Z - handLeft.Position.Z) >= movingThreshold &&
                (oldHandRight.Position.X - handRight.Position.X) >= movingThreshold || (oldHandRight.Position.Y - handRight.Position.Y) >= movingThreshold || (oldHandRight.Position.Z - handRight.Position.Z) >= movingThreshold)
            {
                oldHandLeft = handLeft; oldHandRight = handRight;
                oldHead = head; oldSpine = spine;
                oldShoulderLeft = shoulderLeft; oldShoulderRight = shoulderRight;
                oldHipLeft = hipLeft; oldHipRight = hipRight;
                movingTimeOut = 0;
                return true;
            }
            else
            {
                return false;
            }
        }

        public bool CheckHandStopMoving()
        {
            if ((oldHandLeft.Position.X - handLeft.Position.X) < movingThreshold || (oldHandLeft.Position.Y - handLeft.Position.Y) < movingThreshold || (oldHandLeft.Position.Z - handLeft.Position.Z) < movingThreshold &&
                (oldHandRight.Position.X - handRight.Position.X) < movingThreshold || (oldHandRight.Position.Y - handRight.Position.Y) < movingThreshold || (oldHandRight.Position.Z - handRight.Position.Z) < movingThreshold)
            {
                movingTimeOut += 1;
                if (movingTimeOut > 20)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else
            {
                return false;
            }
        }

        public bool CheckReadyStance()
        {
            if (((handLeft.Position.Y - spine.Position.Y) > 0) || ((handRight.Position.Y - spine.Position.Y) > 0))
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        public float GetHandDistant()
        {
            return (handLeft.Position.Y - spine.Position.Y);
        }

        public bool CheckSkeleton()
        {
            if (first != null)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        #region GET Method

        #region Get Data STRING Type

        public String getHeadData()
        {
            return "Head ["+headX+", " + headY+", " + headZ+"] \n";
        }

        public String getLeftHandData()
        {
            return "LeftHand [" + leftX + ", " + leftY + ", " + leftZ + "] \n";
        }

        public String getRightHandData()
        {
            return "RightHand [" + rightX + ", " + rightY + ", " + rightZ + "] \n";
        }

        public String getLeftShoulderData()
        {
            return "LeftShoulder [" + shoulderLX + ", " + shoulderLY + ", " + shoulderLZ + "] \n";
        }

        public String getRightShoulderData()
        {
            return "RightShoulder [" + shoulderRX + ", " + shoulderRY + ", " + shoulderRZ + "] \n";
        }

        public String getLeftHipData()
        {
            return "LeftHip [" + hipLX + ", " + hipLY + ", " + hipLZ + "] \n";
        }

        public String getRightHipData()
        {
            return "RightHip [" + hipRX + ", " + hipRY + ", " + hipRZ + "] \n";
        }

        #endregion

        #region GET Data Float Type

        public double getHeadPos(String axis)
        {
            if ((axis.ToUpper()).Equals("X"))
            {
                return headX;
            }
            else if ((axis.ToUpper()).Equals("Y"))
            {
                return headY;
            }
            else if ((axis.ToUpper()).Equals("Z"))
            {
                return headZ;
            }
            else
            {
                return 0;
            }
        }

        public double getLeftHandPos(String axis)
        {
            if ((axis.ToUpper()).Equals("X"))
            {
                return leftX;
            }
            else if ((axis.ToUpper()).Equals("Y"))
            {
                return leftY;
            }
            else if ((axis.ToUpper()).Equals("Z"))
            {
                return leftZ;
            }
            else
            {
                return 0;
            }
        }

        public double getRightHandPos(String axis)
        {
            if ((axis.ToUpper()).Equals("X"))
            {
                return rightX;
            }
            else if ((axis.ToUpper()).Equals("Y"))
            {
                return rightY;
            }
            else if ((axis.ToUpper()).Equals("Z"))
            {
                return rightZ;
            }
            else
            {
                return 0;
            }
        }

        public double getLeftShoulderPos(String axis)
        {
            if ((axis.ToUpper()).Equals("X"))
            {
                return shoulderLX;
            }
            else if ((axis.ToUpper()).Equals("Y"))
            {
                return shoulderLY;
            }
            else if ((axis.ToUpper()).Equals("Z"))
            {
                return shoulderLZ;
            }
            else
            {
                return 0;
            }
        }

        public double getRightShoulderPos(String axis)
        {
            if ((axis.ToUpper()).Equals("X"))
            {
                return shoulderRX;
            }
            else if ((axis.ToUpper()).Equals("Y"))
            {
                return shoulderRY;
            }
            else if ((axis.ToUpper()).Equals("Z"))
            {
                return shoulderRZ;
            }
            else
            {
                return 0;
            }
        }

        public double getLeftHipPos(String axis)
        {
            if ((axis.ToUpper()).Equals("X"))
            {
                return hipLX;
            }
            else if ((axis.ToUpper()).Equals("Y"))
            {
                return hipLY;
            }
            else if ((axis.ToUpper()).Equals("Z"))
            {
                return hipLZ;
            }
            else
            {
                return 0;
            }
        }

        public double getRightHipPos(String axis)
        {
            if ((axis.ToUpper()).Equals("X"))
            {
                return hipRX;
            }
            else if ((axis.ToUpper()).Equals("Y"))
            {
                return hipRY;
            }
            else if ((axis.ToUpper()).Equals("Z"))
            {
                return hipRZ;
            }
            else
            {
                return 0;
            }
        }

        #endregion

        #endregion

    }
}
